/*******************************************************************
 *  Based on STM32Cube_FW_F4_V1.25.0 library and rt-thread-v3.1.3  *
 *	Email:1216579118@qq.com                                        *
 *	Author: 哇咔咔的海角                                           *
*******************************************************************/

#include "main.h"
#include "stdlib.h"
#include "arm_math.h"


#define THREAD_PRIORITY 20
#define THREAD_STACK_SIZE 1024
#define THREAD_TIMESLICE 10

#define  printf rt_kprintf


 
//用随机数产生近似高斯白噪声
double gauss_rand() 
{      
  double temp=(rand()/(double)RAND_MAX);
	 return 2*(temp-0.5)*0.03; 
	 
}  
/*****************Kalamn离散状态差分方程*******************
*  X(k)=AX(k-1)+BU(k)+W(k)                                *
*  Z(k)=HX(k)+V(k)                                        *
**********************************************************/


/******************************************************************
*  1.状态预测方程(先验估计)：                         
*    X(k|k-1)=AX(k-1|k-1)+BU(k)
*  2.误差协方差预测矩阵(先验误差协方差)：
*    P(k|k-1)=AP(k-1|k-1)At+Q
*  3.Kalman Gain方程：
*    K(k)=[P(k|k-1)Ht]/[HP(k|k-1)Ht+R]
*  4.最优值更新方程：
*    X(x|x)=X(k|k-1)+K(k)[Z(k)-HX(k|k-1)]
*  5.更新k时刻误差协方差(后验估计):
*    P(k|k)=[1-K(k)H]P(k|k-1)
*  说明：
*    本例程中X(k)先验估计指由状态空间模型在X(k-1)时刻估计的X(k)值;
*            X(k)后验估计指由卡尔曼滤波器4式估计的最优值;
*            P(k)先验估计指由公式2的出的先验误差方差(协方差矩阵),即E[(X(k|k)-X(k|k-1))(X(k|k)-X(k|k-1))t];
*            P(k)后验估计指由公式5得出的后验误差方差(协方差矩阵);
*******************************************************************/

/**

p_now==(1-kg)*p_mid;//K时刻最优值(后验)方差估计(Q为矩阵时为covariance矩阵估计)
p_mid==p_last+Q;    //K时刻的先验方差估计(Q为矩阵时为covariance矩阵估计)
p_last              //K-1时刻最优值(后验)方差估计(Q为矩阵时为covariance矩阵估计)

x_now==x_mid+kg*(z_measure-x_mid);//K时刻最优值(后验)估计
x_mid;                            //x_mid=x(k|k-1)    K时刻的先验估计
x_last=z_real+frand()*0.03;       //x_last=x(k-1|k-2) K-1时刻的先验估计

z_measure=H*X+frand()*0.03=z_real+frand()*0.03;    //H为状态观测系数（矩阵），

系统矩阵     A=1(一维)
输入矩阵     B=0(一维)
状态观测矩阵 H=1(一维)

**/

void kalman_filter()
{	
	float x_last=0;   
	float p_last=0.02;//p_last=p(k-1|k-1)
	float Q=0.018;    //估计方差（协方差矩阵）
	float R=0.542;    //测量方差（协方差矩阵）
	float kg;         //Kalman Gain
	float x_mid;      //x_mid=x(k|k-1) K时刻的先验估计
	float x_now;      //x_now=x(k|k)   K时刻最优值(后验)估计
	float p_mid;  
	float p_now;
	float z_real=0.56;//定义真实值
	float z_measure;  //Z(k)=HX(k)+V(k)  
	float kalman_sum_error=0;  //卡尔曼滤波值与真实值的积分误差
	float measure_sum_error=0; //实际测量值与真实值的积分误差
	int i;
	x_last=z_real+gauss_rand();//递归运算前给出的K-1时刻的后验估计
	x_mid=x_last;//x_mid暂存上一次的最优估计值
	
	for(i=0;i<100;i++)
	{	x_mid=x_last;       //x_mid暂存上一次的最优估计值，由于A=1,B=0,故x_last=x(k-1|k-1)=x_mid=x(k|k-1)
		p_mid=p_last+Q;     //K时刻的先验误差方差估计，Q为状态估计的误差方差(协方差矩阵)     p_mid=p(k|k-1),p_last=p(k-1|k-1)
		
		kg=p_mid/(p_mid+R); //kg为kalman filter，R为测量误差的方差(协方差矩阵)
		
		z_measure=z_real+gauss_rand();//实际测量值
		
		x_now=x_mid+kg*(z_measure-x_mid);//x_now=x(k|k)   K时刻最优值(后验)估计
		
		p_now=(1-kg)*p_mid;//K时刻最优值(后验)方差估计(Q为矩阵时为covariance矩阵估计)
		
		
		printf("Real: %6.3f  Mesaured: %6.3f  [err_R-M:%.3f]  Kalman: %6.3f  [err_R-K:%.3f]\n"\
		       ,z_real,z_measure,fabs(z_real-z_measure),x_now,fabs(z_real - x_now));

		
		kalman_sum_error += fabs(z_real - x_now);  //kalman估计值的累积误差
		measure_sum_error += fabs(z_real-z_measure);  //真值与测量值的累积误差
        p_last = p_now;  //更新covariance值  K-1时刻最优值(后验)方差估计
        x_last = x_now;  //更新系统状态值    x_now=x(k|k)  K时刻最优值(后验)估计
		//rt_thread_mdelay(100);
	}
	// printf("RAND_MAX:%\n",RAND_MAX);
	printf("kalman_sum_error: %f measure_sum_error: %f \n",kalman_sum_error,measure_sum_error); 
	
	rt_thread_mdelay(1000);
	rt_thread_mdelay(1000);
	  // printf("总体测量误差      : %f\n",sumerror_measure);  //输出测量累积误差
    // printf("总体卡尔曼滤波误差: %f\n",sumerror_kalman);   //输出kalman累积误差
    // printf("卡尔曼误差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100)); 
}


/****************************线程入口函数*****************************************/
void kalman_entry(void *param)
{
	 while(1)
	 {
	   kalman_filter();	 
	 }
}

void led1_init(void)
{		
	GPIO_InitTypeDef GPIO_InitStruct = {0};
	__HAL_RCC_GPIOC_CLK_ENABLE();
	
	  /*Configure GPIO pin : PtPin */
  GPIO_InitStruct.Pin = GPIO_PIN_13;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
	
}
void led1_entry(void *param)
{
	led_init();
	 while(1)
	 {
		 
	 HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
	 rt_thread_mdelay(1000);
	 //rt_kprintf("led1_thread running,LED1_ON\r\n");
		
	 HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);	
	 rt_thread_mdelay(1000);
	 }
}

/*****************************用户应用程序***************************/
int kalman_sample(void)
{
	
	//指向指针控制块的指针
    rt_thread_t thread;
	
	//创建名为thread2的线程									
	thread=rt_thread_create("thread2",
	                        led1_entry,
	                        RT_NULL,
							            THREAD_STACK_SIZE,
							            THREAD_PRIORITY,
							            THREAD_TIMESLICE											 
	                       );
	if(thread!=RT_NULL)
		rt_thread_startup(thread);//将线程的状态更改为就绪状态，并放到相应优先级队列中等待调度
  
	thread=rt_thread_create("thread2",
	                        kalman_entry,
	                        RT_NULL,
							            THREAD_STACK_SIZE,
							            THREAD_PRIORITY,
							            THREAD_TIMESLICE-5											 
	                       );
	if(thread!=RT_NULL)
		rt_thread_startup(thread);
	
  return 0;	

}

MSH_CMD_EXPORT(kalman_sample,kalman sample);


/********************************************************************************************************************/
/*
线程睡眠：需要让运行的当前线程延迟一段时间，在指定的
时间到达后重新运行，叫做“线程睡眠”
线程睡眠使用以下三个函数：

rt_err_t rt_thread_sleep(rt_tick_t tick);
rt_err_t rt_thread_delay(rt_tick_t tick);
rt_err_t rt_thread_mdelay(rt_int32_t ms);

这三个函数接口的作用相同，调用它们可以使当前线程挂起一段指定的时间，
当这个时间过后，线程会被唤醒并再次进入就绪状态

*******************************************************************/


/********************************************************
rt_thread_create()创建动态线程
rt_thread_init()初始化一个静态线程

1.动态线程是系统自动从动态内存堆上分配栈空间与线程句柄
（初始化heap之后才能使用create创建动态线程）；静态线程
是由用户分配栈空间与线程句柄（内核创建线程）
分配的栈空间是按照rtconfig.h中配置的RT_ALIGN_SIZE方式对齐
2.脱离线程列表方式
  rt_thread_create() ---> rt_thread_delete()
  rt_thread_init() ----> rt_thread_detach()
********************************************************/


/*******************************************************
RO Size Flash
code:代码段，存放程序的代码部分
RO-data:只读数据段，存放程序中定义的常量const

RW Size RAM
RW-data:读写数据段，存放初始化为非0的全局变量
ZI-data:0数据段，存放未初始化的全局变量及初始化为0的变量

*********************************************************/


















